#include "con_algorithm.h"
/*The following is the pid part code */

/**
 * @description:
 * @param {type}
 * @return:
 */

//extern u8 start_flag;

 /**
  * @description: PID  hander initialization
  * @param  PIDtypedef * PIDx hander
  * @return:
            SUCCESS
            FAIL
  */

 int PIDx_init(PIDtypedef * PIDx)
{
    if(PIDx == NULL)
    {
        return FAIL;
    }
    PIDx->cur_error=0;
    PIDx->last_error=0;
    PIDx->prev_error=0;
    PIDx->Kp=0;
    PIDx->Ki=0;
    PIDx->Kd=0;
    PIDx->setpoint=0;
    return SUCCESS;
}

 /**
  * @description: Set the PID handle target value
  * @param PIDtypedef*PIDx PID hander
  * @param float target value
  * @return:
  *         SUCCESS
            FAIL
  */
 int PIDx_setvalue(PIDtypedef*PIDx,float setvalue)
 {
        if(PIDx == NULL)
        {
            return FAIL;
        }
        PIDx->setpoint=setvalue;
        return SUCCESS;
 }


 /**
  * @description: PID_set  PID hander set its proportion、integration and ifferentiation
  * @param  PIDtypedef*PIDx PID hander
  * @param  kp  proportion
  * @param  ki  integration
  * @param  kd  differentiation
  * @return:
  *         SUCCESS
  *         FAIL
  */
 int PIDx_setparam(PIDtypedef *PIDx,float kp,float ki,float kd)
{
        if(PIDx == NULL)
        {
            return FAIL;
        }
        PIDx->Kp = kp;
        PIDx->Ki = ki;
        PIDx->Kd = kd;
        return SUCCESS;
}


/**
 * @description: PIDx_calc Calculate the incremental
 * @param   PIDtypedef *PIDx PID hander
 * @param   curvale  Current actual value
 * @return:
 *         the incremental
 *
 */
float PIDx_calc(PIDtypedef *PIDx,float curvalue)
{
    static float sum=0;
    float iError,iincpid;
    iError=PIDx->setpoint-curvalue;
    /*
    some code can be added here  ,it's userful to limit the error value in a range based application
    */
    iincpid=PIDx->Kp*(iError-PIDx->last_error)  \
            + PIDx->Ki*iError    \
            + PIDx->Kd*(iError-2*PIDx->last_error+PIDx->prev_error);  //calculate  incremental
    PIDx->prev_error = PIDx->last_error;  //error  update
    PIDx->last_error = iError;
	sum+=iError;
	//printf("sum=%f\r\n",sum);
    return(iincpid);
}



/**
 * @description: A simple test PID  program you can run in console application
 * So,By printing the data, we find that the final feedback of PID makes the measured 
 * value converge to the set target value
 * @param void
 * @return: void
 */
void test_main()
{
    int target = 50;
    float simu_measure = 45;
    float add = 0;
    PIDtypedef PID1;
    PIDx_init(&PID1);
    PIDx_setparam(&PID1,0.5,0.05,0.05);
    PIDx_setvalue(&PID1,target);
    for(int i=0; i<2000;i++)
    {
        add = PIDx_calc(&PID1,simu_measure);
        printf("i %d target :%d  add: %f  simu_measure: %f error[-1]:%f error[-2]:%f \n\r",\
        i,target,add,simu_measure,PID1.last_error,PID1.prev_error);
        simu_measure = add + simu_measure;
    }
}
/*Above is the code for the PID part*/

/*flow  some  nomal algoritnm */

/*
The first value of the int array is up to date
 * @description: a simple queue record some last several vaule
 * @param pointer: array start address
 *        len: length of array
 *        vaule: the vaule need to insert
 * @return: void
 */

void sliding_wind_array(int * pointer,int len ,int value)
{
    if(pointer == NULL || len == 0)
    {
        printf("pointer == NULL || len == 0\n");
        return;
    }
    if(len <1)
    {
        printf("len <1 \n");
        return;
    }
    do
    {
        *(pointer+len-1) = *(pointer+len-2);
        len --;

    }while(len -2>=0);
    *pointer = value;
    return;
}




/*The following is the one dimension  Kalman code */
